#include "mindisobs.h"
#include "getindex3d1.h"

vector<vector<float>> mindisobs(vector<vector<float>> zz,vector<float> gMap,vector<int> nz,vector<float> zmin,vector<float> zmax)
{
    float dx=(zmax[0]-zmin[0])/nz[0];
    float dy=(zmax[1]-zmin[1])/nz[1];
    float dth=(zmax[2]-zmin[2])/nz[2];
    float lf=3.5,lr=3;
    int k=3;
    vector<vector<float>> dis2obs;
    for(int i=0;i<zz.size();i++)
    {
        float rf=100;
        vector<float> dis_temp;
        for(int kk=0;kk<k;k++)
        {
            float xk=zz[i][0]+kk/k*lf*cos(zz[i][2]);
            float yk=zz[i][1]+kk/k*lf*sin(zz[i][2]);
            int idx=ceil((xk-zmin[0])/dx);
            int idy=ceil((yk-zmin[1])/dy);
            int i2d=idx+(idy-1)*nz[0];
            if(idx>0&&idx<nz[0]&&idy>0&&idy<nz[1])
            {
                rf=min(rf,gMap[i2d]);
            }
            else
            {
                rf=0;
            }
        }
        dis_temp.push_back(rf);
        float rb=100;
        for(int kk=1;kk<k;k++)
        {
            float xk=zz[i][0]-kk/k*lf*cos(zz[i][2]);
            float yk=zz[i][1]-kk/k*lf*sin(zz[i][2]);
            int idx=ceil((xk-zmin[0])/dx);
            int idy=ceil((yk-zmin[1])/dy);
            int i2d=idx+(idy-1)*nz[0];
            if(idx>0&&idx<nz[0]&&idy>0&&idy<nz[1])
            {
                rb=min(rf,gMap[i2d]);
            }
            else
            {
                rb=0;
            }
        }
        dis_temp.push_back(rb);
        dis2obs.push_back(dis_temp);

    }
    return dis2obs;
}